/***************************************************
 @Name :        小铭同学
 @Time :        2022-10-30
 @Email :       LiaoMingWJ233@163.com
 @Comment :     底盘循迹控制功能、转弯功能的实现
 @FileName :    track.c
 @Version :     1.0
****************************************************/


#include "track.h"

#include "device.h"


static short flag = 0;

/* 开环前进 */
void drive(track_speed_t s)
{
  motor_move(s, 0);
}


/* 前进循迹 */
void track_drive(track_speed_t s)
{
  motor_move(s, s* 90/100 * gray_front_error());
}

void track_drive1(track_speed_t s)
{
  motor_move(s, s* 90/100 * gray_front_error1());
}

/* 开环倒车 */
void reverse(track_speed_t s)
{
  motor_move(-s, 0);
}

/* 倒车循迹，前后灰度err定义相同，差速值加个负号，逻辑相反 */
void track_reverse(track_speed_t s)
{
  motor_move(-s, -s* 90/100 * gray_back_error());
}

void track_stop(void)
{
  motor_move(0, 0);
}

/* 停在十字路口时 左转 */
void track_turn_left(track_speed_t s)
{

	flag = 1;
	motor_move(0, -s);

  delay_ms(500);

  while (gray_turn_not_ok0());  /* 没转到位，程序停在此处 */

  motor_move(0, 0);		
}

int flag_left()
{
	return flag;
}

int flag_stop()
{
	flag = 0;
}

/* 停在十字路口时 右转 */
void track_turn_right(track_speed_t s)
{
  motor_move(0, s);

  delay_ms(500);

  while (gray_turn_not_ok0());  /* 没转到位，程序停在此处 */

  motor_move(0, 0);
}

/* 循迹测试函数 */
void track_drive_test(void)
{
  while (1)
  {
    /* 20ms执行一次控制 */
    delay_ms(20);

    /* 车前进速度和差速值控制，error前的系数调整转弯力度 */
//    motor_move(300, 1000 * gray_front_error());
    motor_move(300, 260 * gray_front_error());

    /* 遇到十字路口，停车，跳出控制循环 */
    if (gray_is_cross())
    {
      motor_move(0, 0);
      break;
    }
  }

  while (1);   /* 测试程序停在此处 */
}


void track_reverse_test(void)
{
  while (1)
  {
    delay_ms(20);

    track_reverse(speed_medium);

    /* 遇到十字路口，停车，跳出控制循环 */
    if (gray_is_cross())
    {
      motor_move(0, 0);
      break;
    }
  }

  while (1);   /* 测试程序停在此处 */
}


void track_turn_test(void)
{
  while (1)
  {
    /* 20ms执行一次控制 */
    delay_ms(20);

    /* 车前进速度和差速值控制，error前的系数调整转弯力度 */
    motor_move(300, 260 * gray_front_error());

    /* 遇到十字路口，停车，跳出控制循环 */
    if (gray_is_cross())
    {
      motor_move(0, 0);
      break;
    }
  }

  delay_ms(500);

  track_turn_left(speed_medium);

  while (1);
}


